/*!
  @file motion_solver_p3p.cpp
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#include "mrover_vo/motion_solver_p3p.h"

#include <ros/ros.h>
#include <opencv2/calib3d/calib3d.hpp>

namespace vo {

void MotionSolverP3P::compute() {
  if (!initCompute()) {
    ROS_ERROR("error in p3p solver");
    return;
  }

  img_pts_.clear();
  obj_pts_.clear();
  for (Feature::Matches::const_iterator match_it = matches_->begin(); match_it != matches_->end(); ++match_it) {
    img_pts_.push_back(input_->kp_vec[0]->at(match_it->queryIdx).pt);  // at time t-1
    obj_pts_.push_back(target_->sp->at(match_it->trainIdx));    // at time t
  }

  cv::Mat rvec(3, 1, CV_64F);
  cv::Mat t(3, 1, CV_64F);

  if (use_guess_ && pose_.data) {
    cv::Mat R(3, 3, CV_64F);
    for (int i = 0; i < 3; ++i) {
      for (int j = 0; j < 3; ++j) {
        R.at<double>(i, j) = pose_.at<float>(i, j);
      }
      t.at<double>(i, 0) = pose_.at<float>(i, 3);
    }
    cv::Rodrigues(R, rvec);
  }

  inliers_->clear();
  int num_iteration = 100;
  float error_repj = 8.0;
  int method = (use_lm_)? CV_ITERATIVE: CV_P3P;
  cv::solvePnPRansac(obj_pts_, img_pts_, cam_params_->K[0], cam_params_->D[0], 
      rvec, t, use_guess_,num_iteration, error_repj, 100, 
      *inliers_, method);

  cv::Mat R;
  cv::Rodrigues(rvec, R);

  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      pose_.at<float>(i, j) = R.at<double>(i, j);
    }
    pose_.at<float>(i, 3) = t.at<double>(i, 0);
  }
  pose_.at<float>(3, 0) = 0;
  pose_.at<float>(3, 1) = 0;
  pose_.at<float>(3, 2) = 0;
  pose_.at<float>(3, 3) = 1;

  //ROS_INFO_STREAM(pose_); 
  //ROS_INFO("%d inliers / %d", inliers_->size(), matches_->size());

  // reset flags
  if (use_guess_) use_guess_ = false;
  if (use_lm_) use_lm_ = false;
}

}  // namespace vo
